/*
 * @Description: 回环检测具体实现
 * @Author: Sang Hao
 * @Date: 2021-09-27 13:29:29
 * @LastEditTime: 2021-11-09 20:17:13
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_
#define LIDAR_SLAM_MAPPING_LOOP_CLOSING_LOOP_CLOSING_HPP_

#include <deque>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>
// #include <pcl/registration/ndt.h>

#include "lidar_slam/sensor_data/loop_pose.hpp"
#include "lidar_slam/models/cloud_filter/cloud_filter_interface.hpp"
#include "lidar_slam/models/registration/registration_interface.hpp"
#include "lidar_slam/sensor_data/key_frame.hpp"

namespace lidar_slam {
class LoopClosing { 
private:
	/* data，以下值均是默认值，可通过config_node做初始化 */
	// string关键帧路径，用保存的关键帧来做匹配
	std::string key_frames_path_ = "";
	// 关键帧扩展的帧数
	int extend_frame_num_ = 3;
	// 每检测一次，就做一次等待，等待帧数为loop_step_
	int loop_step_ = 10;
	// diff_num_：当距离当前帧在diff_num_帧之内时，不做闭环检测
	int diff_num_ = 100;
	// 检测区域，只有两帧距离小于这个值，才做闭环匹配
	float detect_area_ = 10.0;
	// 匹配误差小于这个值才认为是有效的
	float fitness_score_limit_ = 2.0; 

	std::shared_ptr<CloudFilterInterface> scan_filter_ptr_;
	std::shared_ptr<CloudFilterInterface> map_filter_ptr_;
	std::shared_ptr<RegistrationInterface> registration_ptr_;

	/* 同样，deque，方便了类内成员函数之间的参数传递 */
	std::deque<KeyFrame> all_key_frames_;

	LoopPose current_loop_pose_;
	bool has_new_loop_pose_ = false;

private:
	bool InitWithConfig();
	bool InitParam(const YAML::Node& config_node);
	bool InitDataPath(const YAML::Node& config_node);
	bool InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, 
		const YAML::Node& config_node);
	bool InitFilter(std::string filter_user, 
		std::shared_ptr<CloudFilterInterface>& filter_ptr,
		const YAML::Node& config_node);

	bool DetectNearestKeyFrame(int& key_frame_index);
	bool CloudRegistration(int key_frame_index);
	bool JointMap(int key_frame_index, CloudData::CLOUD_PTR& map_cloud_ptr, 
		Eigen::Matrix4f& map_pose);
	bool JointScan(CloudData::CLOUD_PTR& scan_cloud_ptr, Eigen::Matrix4f& scan_pose);
	bool Registration(	CloudData::CLOUD_PTR& map_cloud_ptr, 
						CloudData::CLOUD_PTR& scan_cloud_ptr,
						Eigen::Matrix4f& scan_pose,
						Eigen::Matrix4f& result_pose	);
	

public:
	LoopClosing(/* args */);
	bool Update(const KeyFrame key_frame);
	bool HasNewLoopPose();
	LoopPose& GetCurrentLoopPose();
	
};
}

#endif